# 版权所有 2025 地球仪 (ldhhp@163.com)
# 本文件基于灯哥的 Py-Apple Dog 项目进行修改。
# 原始项目地址：https://github.com/ToanTech/py-apple-quadruped-robot
# 本项目遵循 Apache 许可证 2.0 版。
# 你可以在以下地址获取许可证副本：http://www.apache.org/licenses/LICENSE-2.0

# 机器狗主控制程序，负责运动控制、姿态调节、步态生成等核心功能

#导入狗数据
import dog_data
import dog_cfg
import wifi_moudle

wifi_moudle.selfadd="0.0.0.0"

from setcfg import DAT

def cal_test_shank(x):
  p1=0.006649
  p2=0.4414
  p3=5.53
  return p1*x*x+p2*x+p3

def servo_output(case,init,leg1_shou,leg1_ham,leg1_shank,leg2_shou,leg2_ham,leg2_shank,leg3_shou,leg3_ham,leg3_shank,leg4_shou,leg4_ham,leg4_shank):
  pass

def height(goal):    #高度调节函数
    dog_cfg.H_goal=goal

def gesture(PIT,ROL,YAW,X):
    dog_cfg.PIT_goal=PIT
    dog_cfg.ROL_goal=ROL
    dog_cfg.YAW_goal=YAW
    dog_cfg.X_goal=X


#快速调节函数（适用于串口）
def g(PIT):
    dog_cfg.PIT_goal=PIT
    
def m(spd_,L_,R_):
    dog_cfg.spd=spd_
    dog_cfg.Left=L_
    dog_cfg.Right=R_
#快速调节函数（适用于串口）


def move(spd_,L_,R_):
    dog_cfg.spd_goal=spd_
    dog_cfg.Left=L_
    dog_cfg.Right=R_
  
def stable(key):
    dog_cfg.key_stab=key
  
def servo_init(key):
    dog_cfg.init_case=key

    
def gait(mode):   #设置步态
  dog_cfg.gait_mode=mode


def read_voltage(x):   #电压拟合
  p1 = 0.002575
  p2 = 0.7446
  return p1*x + p2

def mainloop():
    try:
      #判断步态模式
      if dog_cfg.gait_mode==0:    #TROT 步态
          if dog_cfg.deltaStep>=(dog_cfg.Ts-dog_data.speed):#一个完整的运动周期结束 trot
              dog_cfg.deltaStep=0
          elif dog_cfg.Left==0 and dog_cfg.Right==0:
              dog_cfg.deltaStep=0
          else:
              dog_cfg.deltaStep=dog_cfg.deltaStep+dog_data.speed
          
          P_=PA_TROT.cal_t(dog_cfg.deltaStep,0,dog_cfg.spd*8,dog_data.raiseH,dog_cfg.Left,dog_cfg.Left,dog_cfg.Right,dog_cfg.Right)
      elif dog_cfg.gait_mode==2:    #平移步态
          if dog_cfg.deltaStep>=(dog_cfg.Ts-dog_data.speed):#一个完整的运动周期结束 trot
              dog_cfg.deltaStep=0
          elif dog_cfg.Left==0 and dog_cfg.Right==0:
              dog_cfg.deltaStep=0
          else:
              dog_cfg.deltaStep=dog_cfg.deltaStep+dog_data.speed
          P_=PA_TLT.cal_tlt(dog_cfg.deltaStep,0,dog_cfg.spd*4,dog_data.raiseH,dog_cfg.Left,dog_cfg.Left,dog_cfg.Right,dog_cfg.Right)
      #速度调节器
      if dog_cfg.spd>dog_cfg.spd_goal:
          dog_cfg.spd=dog_cfg.spd-abs(dog_cfg.spd-dog_cfg.spd_goal)*dog_cfg.Kp_V
      elif dog_cfg.spd<dog_cfg.spd_goal:
          dog_cfg.spd=dog_cfg.spd+abs(dog_cfg.spd-dog_cfg.spd_goal)*dog_cfg.Kp_V
      #高度调节器1
      if dog_cfg.R_H>dog_cfg.H_goal:
          dog_cfg.R_H=dog_cfg.R_H-abs(dog_cfg.R_H-dog_cfg.H_goal)*dog_data.Kp_H
      elif dog_cfg.R_H<dog_cfg.H_goal:
          dog_cfg.R_H=dog_cfg.R_H+abs(dog_cfg.R_H-dog_cfg.H_goal)*dog_data.Kp_H
      #姿态调节器
      if dog_cfg.PIT_S>dog_cfg.PIT_goal:   #俯仰
          dog_cfg.PIT_S=dog_cfg.PIT_S-abs(dog_cfg.PIT_S-dog_cfg.PIT_goal)*dog_data.Kp_G
      elif dog_cfg.PIT_S<dog_cfg.PIT_goal:
          dog_cfg.PIT_S=dog_cfg.PIT_S+abs(dog_cfg.PIT_S-dog_cfg.PIT_goal)*dog_data.Kp_G
  
      if dog_cfg.ROL_S>dog_cfg.ROL_goal:   #滚转
          dog_cfg.ROL_S=dog_cfg.ROL_S-abs(dog_cfg.ROL_S-dog_cfg.ROL_goal)*dog_data.Kp_G
      elif dog_cfg.ROL_S<dog_cfg.ROL_goal:
          dog_cfg.ROL_S=dog_cfg.ROL_S+abs(dog_cfg.ROL_S-dog_cfg.ROL_goal)*dog_data.Kp_G
          
      if dog_cfg.YAW_S>dog_cfg.YAW_goal:   #偏航
          dog_cfg.YAW_S=dog_cfg.YAW_S-abs(dog_cfg.YAW_S-dog_cfg.YAW_goal)*dog_data.Kp_G
      elif dog_cfg.YAW_S<dog_cfg.YAW_goal:
          dog_cfg.YAW_S=dog_cfg.YAW_S+abs(dog_cfg.YAW_S-dog_cfg.YAW_goal)*dog_data.Kp_G

      if dog_cfg.X_S>dog_cfg.X_goal:   #X位置
         dog_cfg.X_S=dog_cfg.X_S-abs(dog_cfg.X_S-dog_cfg.X_goal)*dog_data.Kp_G
      elif dog_cfg.X_S<dog_cfg.X_goal:
          dog_cfg.X_S=dog_cfg.X_S+abs(dog_cfg.X_S-dog_cfg.X_goal)*dog_data.Kp_G

      #姿态角度限位  
      if dog_cfg.PIT_S>=dog_cfg.pit_max_ang:dog_cfg.PIT_S=dog_cfg.pit_max_ang
      if dog_cfg.PIT_S<=-dog_cfg.pit_max_ang:dog_cfg.PIT_S=-dog_cfg.pit_max_ang
      if dog_cfg.ROL_S>=dog_cfg.rol_max_ang:dog_cfg.ROL_S=dog_cfg.rol_max_ang
      if dog_cfg.ROL_S<=-dog_cfg.rol_max_ang:dog_cfg.ROL_S=-dog_cfg.rol_max_ang
      #TROT模态根据迈腿长度自动调节重心
      if dog_cfg.gait_mode==0:
          if dog_cfg.spd>=0 and (dog_cfg.Left+dog_cfg.Right)!=0:              
              P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S-abs(dog_cfg.spd)*dog_data.trot_cg_f,dog_cfg.R_H)
          elif dog_cfg.spd<0 and (dog_cfg.Left+dog_cfg.Right)!=0:
              P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S+abs(dog_cfg.spd)*dog_data.trot_cg_b,dog_cfg.R_H)
          elif (dog_cfg.Left+dog_cfg.Right)==0:    #原地旋转
              P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S+abs(dog_cfg.spd)*dog_data.trot_cg_t,dog_cfg.R_H)
      else:
          P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S,dog_cfg.R_H)
          
      dog_cfg.ges_x_1=P_G[0];dog_cfg.ges_x_2=P_G[1]; dog_cfg.ges_x_3=P_G[2];  dog_cfg.ges_x_4=P_G[3]
      dog_cfg.ges_y_1=P_G[4];dog_cfg.ges_y_2=P_G[5]; dog_cfg.ges_y_3=P_G[6];  dog_cfg.ges_y_4=P_G[7]
      dog_cfg.ges_z_1=P_G[8];dog_cfg.ges_z_2=P_G[9]; dog_cfg.ges_z_3=P_G[10]; dog_cfg.ges_z_4=P_G[11]
    except Exception as ee:
      print("mainloop :"+str(ee))

import PA_ATTITUDE
import PA_IK

P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,223,95,160,dog_cfg.X_S,dog_cfg.R_H)
print(P_G)

#lL=[80,84,34]
#A_=PA_IK.ik(dog_data.ma_case,lL,P_G[0],P_G[1],P_G[2],P_G[3],P_G[4],P_G[5],P_G[6],P_G[7],P_G[8],P_G[9],P_G[10],P_G[11])
#print(A_)
#j = dog_data.parts[0][0]+A_[0]
#d = dog_data.parts[0][1]+90-(90-A_[1])
#x = dog_data.parts[0][2]-90+cal_test_shank(-A_[2])
#
#print(j,d,x,cal_test_shank(-A_[2]))
